#include "maths/plane.h"
#include "maths/utils.h"

namespace dw {
	namespace maths {

Plane::Plane(const float& a, const float& b, const float& c, const float& d) :
	a(a), b(b), c(c), d(d)
{
	this->d /= this->normal.length();
	this->normal.normalize();
}

																	Plane::Plane(const Vector3f& n, const float& d) : normal(n), d(d)
{
	this->d /= this->normal.length();
	this->normal.normalize();
}

																	Plane::Plane(const Vector3f& n, const Point3f& p) : normal(n)
{
	this->normal.normalize();
	this->d = this->a * p.x + this->b * p.y + this->c * p.z;
}

																	Plane::Plane(const Point3f& p0, const Point3f& p1, const Point3f& p2)
{
	this->normal = Vector3f::cross(p1 - p0, p2 - p0);
	this->normal.normalize();
	this->d = this->a * p0.x + this->b * p0.y + this->c * p0.z;
}

Plane Plane::transform(const Plane& p, const Matrix<float, 4, 4>& m)
{
	Vector4f v(p.a, p.b, p.c, p.d);

	v = m * v;
	return Plane(v.x, v.y, v.z, v.w);
}

float Plane::distance(const Plane& plane, const Point3f& point)
{
	return plane.a * point.x + plane.b * point.y + plane.c * point.z + plane.d;
}

bool Plane::intersection(const Plane& plane0, const Plane& plane1, const Plane& plane2, Point3f& intersection)
{
	Matrix44f m(Vector4f(plane0.a, plane0.b, plane0.c, plane0.d), 
							Vector4f(plane1.a, plane1.b, plane1.c, plane1.d),
							Vector4f(plane2.a, plane2.b, plane2.c, plane2.d),
							Vector4f(0, 0, 0, 1));

	Matrix44f::inverse(m);

	if (m == Matrix44f::zero)
		return false;
	else
	{
		intersection = Point3f(m[0, 3], m[1, 3], m[2, 3]);
		return true;
	}
}

	} // namespace maths
} // namespace dw
